/*************************************************************************
	> File Name: modbus.cpp
	> Author: xuwenlong
	> Mail: myxuan475@126.com 
	> Created Time: 2018年12月06日 星期四 10时28分23秒
 ************************************************************************/
#include "modbus.h"
#include <utils/custom.h>
#include <acquisition/acquisition.h>

#define U16toBG(buf,v) {\
    (buf)[0] = (U8)((v&0xff00)>>8);\
    (buf)[1] = (U8)(v&0x00ff);\
    }
#define BGtoU16(buf) (((U16)(buf)[0])<<8|((buf)[1]))

Modbus::Modbus():
    m_busLock(true),
    m_iLen(0)
{

}

Modbus::~Modbus()
{

}

void Modbus::AcqDevice()
{
    while (m_iLen >= 11) {
        ModbusData get;
        U16 err = 0;

        err = getFormatData(get);

        if (err != DAS_SUCCESS) {
            sendError(get.id,get.addr,get.code,err);
            continue;
        }

        AcquisitionManager *acqMgr =
                (AcquisitionManager*)GetTask(DAS_ACQUISITOR);

        U8 *out = NULL;
        S32 olen = 0;


        if (get.code == 3) {
            err = acqMgr->acquireDevice(
                        get.addr,get.reg,
                        get.len,out,olen);
        }
        else if (get.code == 1) {
            err = acqMgr->acquireList(out,olen);
        }
        if (err != 0) {
            sendError(get.id,get.addr,get.code,err);
        }
        else {
            sendResult(get.id,get.addr,get.code,out,olen);
            if (olen>0)
                free(out);
        }
    }
}

void Modbus::OnReceiveData(void *data, S32 bytes)
{
    LOG_DEBUG("%s",(char*)data);
    AutoLock lock(m_busLock);
    S32 i = 0;
    while (bytes > 0 && m_iLen+bytes >= 256) {
        S32 len = 256-m_iLen;
        memcpy(&m_busBuffer[m_iLen],&((char*)data)[i],len);
        i += len;
        m_iLen += len;
        bytes -= len;

        AcqDevice();
    }
    memcpy(&m_busBuffer[m_iLen],&((char*)data)[i],bytes);
    m_iLen += bytes;

    AcqDevice();
}

void Modbus::sendError(U16 id,U16 addr,U8 code,U16 err)
{
    U8 result[11];
    U16toBG(result,id);
    U16toBG(&result[3],addr);
    result[2] = code|0xD0;
    result[5] = 0;
    result[6] = 2;
    U16toBG(&result[7],err);

    U16 crc = calcCRC16(result,9);
    U16toBG(&result[9],crc);
    SendData((char*)result,11);
}

void Modbus::sendResult(U16 id,U16 addr, U8 code, U8 *data, U16 olen)
{
    U8 *result = (U8*)malloc(9+olen);
    U16toBG(result,id);
    U16toBG(&result[3],addr);
    result[2] = code;
    U16toBG(&result[5],olen);
    if (olen > 0)
        memcpy(&result[7],data,olen);
    U16 crc = calcCRC16(result,7+olen);
    memcpy(&result[7+olen],&crc,2);
    SendData((char*)result,9+olen);
    free(result);
}

U16 Modbus::getFormatData(ModbusData &get)
{
    int i = 0;
    get.code = 0;

    for (i = 2;i < m_iLen;i++) {
        get.code = m_busBuffer[i];
        if (get.code == 1 || get.code == 3 || get.code == 4)
            break;
    }

    //如果不存在功能码,保留两个字节
    if (m_iLen-i <= 8) {
        memmove(m_busBuffer,&m_busBuffer[i-2],m_iLen-i+2);
        m_iLen = m_iLen-i+2;
        return DAS_INVALID;
    }

    //计算CRC16
    U16 crc,crc_calc;
    memcpy(&crc,&m_busBuffer[i+7],2);

    crc_calc = calcCRC16(&m_busBuffer[i-2],9);
    if (crc != crc_calc) {
        memmove(m_busBuffer,&m_busBuffer[i-1],m_iLen-i+1);
        m_iLen = m_iLen-i+1;
        return DAS_INVALID;
    }

    get.id = BGtoU16(&m_busBuffer[i-2]);
    get.addr = BGtoU16(&m_busBuffer[i+1]);
    memcpy(get.reg,&m_busBuffer[i+3],2);
//    get.reg = BGtoU16(&m_busBuffer[i+3]);
    get.len = BGtoU16(&m_busBuffer[i+5]);

    memmove(m_busBuffer,&m_busBuffer[i+9],m_iLen-i-9);
    m_iLen = m_iLen-i-9;
    return DAS_SUCCESS;
}

U16 Modbus::calcCRC16(U8 usData[], U16 len)
{
    U16 wCRCin = 0xffff;
    U16 wCPoly = 0x8005;
    BOOL low = true;
    U8  wChar = 0;

    for(int i = 0;i < len;i++)
    {
          wChar = usData[i];
          if (low) {
              utils::InvertUint8(&wChar,&wChar);
          }
          wCRCin ^= wChar << 8;
//              wCRCin ^= wEnc;
          for(int i = 0;i < 8;i++)
          {
            if(wCRCin & 0x8000)
              wCRCin = ((wCRCin << 1)) ^ wCPoly;
            else
              wCRCin = (wCRCin << 1);
          }
    }
    if (low)
        utils::InvertUint16(&wCRCin,&wCRCin);

    wCRCin ^= (U16)0x0000;

    return wCRCin;
}
